//
// Created by wzc on 2023/2/6.
//
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>

#include "plan_env/sdf_map.h"
#include "plan_env/map_ros.h"
#include "plan_env/edt_environment.h"

#include "path_searching/astar2.h"

using namespace fast_planner;

Eigen::Vector3d start_pt_;
Eigen::Vector3d goal_pt_;

shared_ptr<SDFMap> sdf_map_;
EDTEnvironment::Ptr edt_environment_;
unique_ptr<Astar> path_finder_;

ros::Publisher path_pub_;
ros::Publisher visited_pt_pub_;

nav_msgs::Path path_;
vector<Eigen::Vector3d> path_vector_;

vector<Eigen::Vector3d> visited_pt_;

Eigen::Vector3d odom;

std::vector<std::string> result_str;

void init(ros::NodeHandle& nh){
  sdf_map_.reset(new SDFMap);
  sdf_map_->initMap(nh);
  edt_environment_.reset(new EDTEnvironment);
  edt_environment_->setMap(sdf_map_);

  path_finder_.reset(new Astar);
  path_finder_->init(nh, edt_environment_);

  sdf_map_->setGlobalKnownFree();

  Eigen::Vector3d obs_min;
  Eigen::Vector3d obs_max;

  /// surroundings
  obs_min = Eigen::Vector3d(-20.0, -10.0, -0.5);
  obs_max = Eigen::Vector3d(20.0, -9.5, 3.0);
  sdf_map_->setObs(obs_min, obs_max);

  obs_min = Eigen::Vector3d(-20.0, 9.5, -0.5);
  obs_max = Eigen::Vector3d(20.0, 10.0, 3.0);
  sdf_map_->setObs(obs_min, obs_max);

  obs_min = Eigen::Vector3d(-20.0, -10.0, -0.5);
  obs_max = Eigen::Vector3d(-19.5, 10.0, 3.0);
  sdf_map_->setObs(obs_min, obs_max);

  obs_min = Eigen::Vector3d(19.5, -10.0, -0.5);
  obs_max = Eigen::Vector3d(20.0, 10.0, 3.0);
  sdf_map_->setObs(obs_min, obs_max);

  Eigen::Vector2d obs_center, obs_size;
  obs_size = Eigen::Vector2d(2.0, 2.0);

  obs_center = Eigen::Vector2d(9.0, 6.0);
  sdf_map_->setObsWithCenter(obs_center, obs_size);

  obs_center = Eigen::Vector2d(5.0, 0.0);
  sdf_map_->setObsWithCenter(obs_center, obs_size);

  obs_center = Eigen::Vector2d(13.0, -1.0);
  sdf_map_->setObsWithCenter(obs_center, obs_size);

  obs_center = Eigen::Vector2d(0.0, -4.0);
  sdf_map_->setObsWithCenter(obs_center, obs_size);

  obs_center = Eigen::Vector2d(-2.0, 3.0);
  sdf_map_->setObsWithCenter(obs_center, obs_size);

  obs_center = Eigen::Vector2d(-10.0, 5.0);
  sdf_map_->setObsWithCenter(obs_center, obs_size);

  obs_center = Eigen::Vector2d(-6.0, -1.0);
  sdf_map_->setObsWithCenter(obs_center, obs_size);

  obs_center = Eigen::Vector2d(-16.0, -0.0);
  sdf_map_->setObsWithCenter(obs_center, obs_size);

  sdf_map_->updateGlobalESDF3d();
  sdf_map_->setAllBoundToGlobal();

  odom << 0.0, 0.0, 0.5;
  start_pt_ << 0.0, 0.0, 0.6;
  path_.header.frame_id = "world";

  result_str = {"REACH_END", "NO_PATH"};
}

void goalCallback(const geometry_msgs::PoseStampedConstPtr& goal){
  goal_pt_[0] = goal->pose.position.x;
  goal_pt_[1] = goal->pose.position.y;
  goal_pt_[2] = 1.8;
  start_pt_ = odom;

  std::cout << "the start pt is : " << start_pt_.transpose() << std::endl;
  std::cout << "the goal is : " << goal_pt_.transpose() << std::endl;

  path_finder_->reset();

  ros::Time t1 = ros::Time::now();
  int res = path_finder_->search(start_pt_,goal_pt_);
  ros::Time t2 = ros::Time::now();
  ROS_WARN_STREAM("time consumption " << (t2 - t1).toSec());

  std::cout << "the search result is : " << result_str[res-1] << std::endl;
  std::cout << "the path size is : " << path_finder_->getPath().size() << std::endl;

  path_vector_.clear();
  path_vector_ = path_finder_->getPath();

  path_.poses.clear();
  for(auto & i : path_vector_){
    geometry_msgs::PoseStamped pt;
    pt.pose.position.x = i(0);
    pt.pose.position.y = i(1);
    pt.pose.position.z = i(2);
    pt.pose.orientation.x = 0;
    pt.pose.orientation.y = 0;
    pt.pose.orientation.z = 0;
    pt.pose.orientation.w = 1;

    path_.poses.push_back(pt);
  }

  visited_pt_.clear();
  visited_pt_ = path_finder_->getVisited();
  std::cout << "visited point size is " << visited_pt_.size() << std::endl;
  std::cout << "============================" << std::endl;
}

void odomCallback(const nav_msgs::OdometryConstPtr &msg) {
  odom[0] = msg->pose.pose.position.x;
  odom[1] = msg->pose.pose.position.y;
  odom[2] = msg->pose.pose.position.z;
}


void timerCallback(const ros::TimerEvent&){
  path_pub_.publish(path_);

  pcl::PointCloud<pcl::PointXYZ> cloud;
  pcl::PointXYZ pt;
  for(int i = 0; i < visited_pt_.size(); i++){
    pt.x = visited_pt_[i](0);
    pt.y = visited_pt_[i](1);
    pt.z = visited_pt_[i](2);
    cloud.push_back(pt);
  }
  cloud.width = cloud.points.size();
  cloud.height = 1;
  cloud.is_dense = true;
  cloud.header.frame_id = "world";
  sensor_msgs::PointCloud2 cloud_msg;
  pcl::toROSMsg(cloud, cloud_msg);

  visited_pt_pub_.publish(cloud_msg);
}



int main(int argc, char** argv)
{
  ros::init(argc,argv,"astar2_test");
  ros::NodeHandle nh("~");

  init(nh);

  ros::Subscriber goal_sub = nh.subscribe("/move_base_simple/goal",10,goalCallback);
  ros::Subscriber odom_sub = nh.subscribe("/t265/odom/sample", 10, odomCallback);

  ros::Timer path_timer = nh.createTimer(ros::Duration(0.2), timerCallback);

  path_pub_ = nh.advertise<nav_msgs::Path>("/astar2/path",10);
  visited_pt_pub_ = nh.advertise<sensor_msgs::PointCloud2>("/astar2/visited_pt",10);

  ros::spin();
}